//
// Created by 13562 on 2023/6/23.
//

#include "iostream"
#include "iomanip"
#include "CLevelingAdjust.h"
#include "utils.h"


void CLevelingAdjust::inputData(const string& knownPointFile, const string& observationDataFile) {
	string _;

	//	读取观测高差数据
	ifstream obsFS(observationDataFile);
	if (!obsFS.is_open()) {
		cerr << "\n 无法打开观测数据文件" << endl;
		exit(-1);
	}

	getline(obsFS, _);
	getline(obsFS, _);
	// 元数据
	obsFS >> m_LNumber >> m_PNumber >> m_sigma;
	getline(obsFS, _);
	getline(obsFS, _);
	getline(obsFS, _);

	// 为各成员变量分配空间
	// 高差起点, 终点号
	startPoints = vector<unsigned int>(m_LNumber, -1);
	endPoints = vector<unsigned int>(m_LNumber, -1);
	// 观测值向量
	L = MatrixXd::Zero(m_LNumber, 1);
	// 观测值权阵
	P = MatrixXd::Zero(m_LNumber, m_LNumber);
	// 高程值向量, 初始值赋为 -9999 表示高程未知
	Hts = MatrixXd::Ones(m_PNumber, 1)*(-9999);
	// 已知点标记全部设置为 false
	knownFlags = vector<bool>(m_PNumber, false);

	Segment segment;
	unsigned int heightIndex = 0;
	while (obsFS >> _ >> segment.startPointName >> segment.endPointName >> segment.segmentHeight >> segment.segmentLength >> _) {
		// 测段数据
		segments.push_back(segment);
		// 建立点号映射
		pair<map<string, unsigned int >::iterator, bool> insertSuccessFlag;
		insertSuccessFlag = pointMap.insert(pair<string, unsigned int>(segment.startPointName, heightIndex));
		if (insertSuccessFlag.second) heightIndex++;
		insertSuccessFlag = pointMap.insert(pair<string, unsigned int>(segment.endPointName, heightIndex));
		if (insertSuccessFlag.second) heightIndex++;
	}

	if (m_LNumber != segments.size()) {
		cerr << "测段总数与实际读入测段数不符！" << endl;
		exit(-1);
	}

	for (int i = 0; i < m_LNumber; i++) {
//		建立起点, 终点号数组
		auto spnIter = pointMap.find(segments.at(i).startPointName);
		if (spnIter != pointMap.end()) {
			startPoints[i] = spnIter->second;
		}
		auto epnIter = pointMap.find(segments.at(i).endPointName);
		if (epnIter != pointMap.end()) {
			endPoints[i] = epnIter->second;
		}
//		输入观测值, 构建观测值权阵
		L(i) = segments.at(i).segmentHeight / 1000;
		P(i, i) = 1 / segments.at(i).segmentLength * 1000;
//		建立高差序号索引
		heightMap.insert(pair(pair(spnIter->first, epnIter->first), i));
	}
	obsFS.close();
//	读取已知高程数据
	ifstream knpFS(knownPointFile);
	if (!knpFS.is_open()) {
		cerr << "\n 无法打开已知点文件" << endl;
		exit(-1);
	}
	getline(knpFS, _);
	string pointName;
	double knHeight;
	while (knpFS >> pointName >> knHeight) {
		// 获取点号
		auto ptIter = pointMap.find(pointName);
		if (ptIter != pointMap.end()) {
			// 设置对应点为已知高程
			Hts(ptIter->second) = knHeight;
			// 设置已知点标记
			knownFlags.at(ptIter->second) = true;
			m_knNumber++;
		}
	}
	knpFS.close();
}

void CLevelingAdjust::printPointMap() {
	cout << "点号映射(点名-点号): " << endl;
	unsigned int col = 1;
	for (const auto& pair : pointMap) {
		cout << pair.first << "-" << pair.second;
		if (col > 4) {
			col = 1;
			cout << endl;
		} else {
			col++;
			cout << "  ";
		}
	}
}

void CLevelingAdjust::printHeightMap() {
	cout << "高差映射: " << endl;
	for (const auto& pair : heightMap) {
		cout << "  起点  终点  高差序号  " << endl;
		cout << setw(4) << pair.first.first;
		cout << setw(4) << pair.first.second;
		cout << setw(6) << pair.second << endl;
	}
}

void CLevelingAdjust::calcApproxHeight() {
	// 记录已计算的近似高程个数
	unsigned int okCount = 0;
	for (int iterCount = 0; ; iterCount++) {
		for (int i = 0; i < m_LNumber; i++) {
			Segment sgm = segments.at(i);
			// 高差起点, 终点号
			unsigned int sptNo = pointMap.find(sgm.startPointName)->second;
			unsigned int eptNo = pointMap.find(sgm.endPointName)->second;
			// 如果起点为已知, 终点为未知
			if (Hts(sptNo) != -9999 && Hts(eptNo) == -9999) {
				Hts(eptNo) = Hts(sptNo) + L(i);
				okCount++;
			}
				// 如果起点未知, 终点已知
			else if (Hts(sptNo) == -9999 && Hts(eptNo) != -9999) {
				Hts(sptNo) = Hts(eptNo) - L(i);
				okCount++;
			}
		}
		// 已计算点数等于未知点数则退出
		if (okCount == (m_PNumber - m_knNumber)) break;
		// 迭代次数大于未知点数则计算近似高程失败
		if (iterCount > (m_PNumber - m_knNumber)) {
			cout << "以下点无法计算近似高程: " << endl;
			for (int i = 0; i < m_PNumber; i++) {
				if (Hts(i) == -9999) cout << "第 " << i << " 点" << endl;
			}
			printPointMap();
			exit(-1);
		}
	}
}

CLevelingAdjust::CLevelingAdjust() {

}

void CLevelingAdjust::printApproxHeights() {
	cout << "概略高程:" << endl;
	cout << "  点名  " << " 映射点号 " << " 概略高程 m" << "  属性 " << endl;
	for (const auto& pair : pointMap) {
		cout << setw(6) << right << pair.first;
		cout << setw(6) << right << pair.second;
		cout << setw(10) << right << fixed << setprecision(3) << sslr(Hts(pair.second), 3);
		cout << setw(8) << right << (knownFlags.at(pair.second) ? "已知":"近似");
		cout << endl;
	}

	cout << "观测高差: " << endl;
	cout << "  起点  终点  映射序号  观测高差 m " << endl;
	for (const auto& pair : heightMap) {
		cout << setw(6) << right << pair.first.first;
		cout << setw(6) << right << pair.first.second;
		cout << setw(6) << right << pair.second;
		cout << setw(10) << right << L(pair.second);
		cout << endl;
	}
}

void CLevelingAdjust::adjust() {
	unsigned int uknNumber = 0;
	// 定义平差参数向量近似值 X0
	VectorXd X0 = VectorXd::Zero(m_PNumber - m_knNumber);
	for (const auto &pair : pointMap) {
		// 在点映射中遍历, 如果为未知点, 则将其高程添加进近似参数
		if (!knownFlags[pair.second]) {
			paraMap.insert(::pair < unsigned int, unsigned int > (pair.second, uknNumber));
			X0(uknNumber) = Hts(pair.second);
			uknNumber++;
		}
	}
	// 组建系数阵B, 常数向量l
	MatrixXd B = MatrixXd::Zero(m_LNumber, m_PNumber - m_knNumber);
	VectorXd d = VectorXd::Zero(m_LNumber);
	for (const auto & pair : heightMap) {
		unsigned int hdNo = pair.second;
		unsigned int spNo = startPoints[hdNo];
		unsigned int epNo = endPoints[hdNo];
		// 如果观测高差起点为已知点, 对应d为已知高程*-1
		// 如果观测高差起点为未知点, 对应B元素为-1
		if (knownFlags.at(spNo)) {
			d(hdNo) = -Hts(spNo);
		} else {
			unsigned int prNo = paraMap.find(spNo)->second;
			B(hdNo, prNo) = -1;
		}
		// 如果观测高差终点为已知点, 对应d为已知高程
		// 如果观测高差终点为未知点, 对应B元素为1
		if (knownFlags.at(epNo)) {
			d(hdNo) = Hts(epNo);
		} else {
			unsigned int prNo = paraMap.find(epNo)->second;
			B(hdNo, prNo) = 1;
		}
	}

	// 计算常向量l
	VectorXd l = L - (B*X0 + d);
	// 计算法方程系数阵
	MatrixXd Nbb = B.transpose()*P*B;
	// 计算法方程逆阵(平差值协因数阵)
	Qxx = Nbb.inverse();
	// 计算参数
	x = Qxx*(B.transpose()*P*l);
	// 计算改正数
	V = B*x - l;
	// 计算参数平差值(高程平差值)
	X = X0 + x;
	// 计算后验单位权中误差
	m_mu = sqrt((V.transpose()*P*V)[0]/uknNumber);
	// 高差平差值协因数阵
	QLL = B*Qxx*B.transpose();
}

void CLevelingAdjust::report(const string& reportFile) {
	ofstream reportFS(reportFile);
	stringstream reportSS;
	reportSS << "=====================METADATA=====================" << endl;
	reportSS << "观测值总数: " << m_LNumber << endl;
	reportSS << "总点数: " << m_PNumber << endl;
	reportSS << "已知点数: " << m_knNumber << endl;
	reportSS << fixed << setprecision(6) << "先验单位权中误差 σ=" << m_sigma << " mm" << endl;
	reportSS << fixed << setprecision(6) << "后验单位权中误差 μ=" << m_mu*1000 << " mm" << endl;
	reportSS << fixed << setprecision(6) << "残差平方和 pvv=" << V.transpose()*P*V << " m²" << endl;
	reportSS << "点号映射(点名-点号): " << endl;
	unsigned int col = 1;
	for (const auto& pair : pointMap) {
		reportSS << pair.first << "-" << pair.second;
		if (col > 4) {
			col = 1;
			reportSS << endl;
		} else {
			col++;
			reportSS << "  ";
		}
	}
	reportSS << "参数号映射(点号-参数号): " << endl;
	col = 1;
	for (const auto& pair : paraMap) {
		reportSS << pair.first << "-" << pair.second;
		if (col > 4) {
			col = 1;
			reportSS << endl;
		} else {
			col++;
			reportSS << "  ";
		}
	}
	reportSS << endl;
	reportSS << "====================高程平差值及其精度====================" << endl;
	reportSS << "  点名  属性   近似高程 m  改正数 mm  高程平差值 m   中误差 mm" << endl;
	for (const auto& pair : pointMap) {
		unsigned int ptNo = pair.second;
		reportSS << setw(6) << right << pair.first;
		if (!knownFlags.at(ptNo)) {
			auto ptIter = paraMap.find(ptNo);
			if (ptIter != paraMap.end()) {
				reportSS << setw(10) << right << "平差点";
				reportSS << fixed << setprecision(5) << setw(10) << right << Hts(ptNo);
				reportSS << fixed << setprecision(5) << setw(10) << right << x(ptIter->second)*1000;
				reportSS << fixed << setprecision(5) << setw(10) << right << X(ptIter->second);
				reportSS << fixed << setprecision(2) << setw(10) << right << m_mu*sqrt(Qxx(ptIter->second, ptIter->second))*1000;
			}
		} else {
			reportSS << setw(10) << right << "已知点";
			reportSS << fixed << setprecision(5) << setw(10) << right << Hts(ptNo);
			reportSS << fixed << setprecision(5) << setw(10) << right << 0.0000;
			reportSS << fixed << setprecision(5) << setw(10) << right << Hts(ptNo);
			reportSS << fixed << setprecision(2) << setw(10) << right << 0.0000;
		}
		reportSS << endl;
	}
	reportSS << "=======================高差平差值及其精度===========================" << endl;
	reportSS << "  起点  终点   观测高差 m  改正数 mm  高差平差值 m   观测权     中误差 mm" << endl;
	for (const auto& segment : segments) {
		unsigned int htNo = heightMap.find(pair(segment.startPointName, segment.endPointName))->second;
		reportSS << setw(6) << right << segment.startPointName;
		reportSS << setw(6) << right << segment.endPointName;
		reportSS << fixed << setprecision(5)<< setw(10) << right << L(htNo);
		reportSS << fixed << setprecision(5)<< setw(10) << right << V(htNo)*1000;
		reportSS << fixed << setprecision(5)<< setw(10) << right << L(htNo) + V(htNo);
		reportSS << fixed << setprecision(2)<< setw(10) << right << P(htNo, htNo);
		reportSS << fixed << setprecision(2)<< setw(10) << right << m_mu*sqrt(QLL(htNo, htNo))*1000;
		reportSS << endl;
	}
	reportFS << reportSS.str();
}

void CLevelingAdjust::printParaMap() {
	cout << "参数号映射(点号-参数号): " << endl;
	unsigned int col = 1;
	for (const auto& pair : paraMap) {
		cout << pair.first << "-" << pair.second;
		if (col > 4) {
			col = 1;
			cout << endl;
		} else {
			col++;
			cout << "  ";
		}
	}
}

CLevelingAdjust::~CLevelingAdjust() = default;

